﻿using OnDash.Mathematics.Matrix;
using OnDash.Mathematics.Vector;

namespace OnDash.Mathematics.Data;

public struct Quaternion : IEquatable<Quaternion>
{
    private Vector3 _xyz;

    public Vector3 Xyz
    {
        get => _xyz;
        set => _xyz = value;
    }

    private float _w;

    public float W
    {
        get => _w;
        set => _w = value;
    }

    public Quaternion(Vector3 xyz, float w)
    {
        _xyz = xyz;
        _w = w;
    }

    public Quaternion(float x, float y, float z, float w) : this(new Vector3(x, y, z), w)
    {
    }

    public Quaternion(float rotationX, float rotationY, float rotationZ)
    {
        rotationX *= 0.5f;
        rotationY *= 0.5f;
        rotationZ *= 0.5f;

        var c1 = MathHelper.Cos(rotationX);
        var c2 = MathHelper.Cos(rotationY);
        var c3 = MathHelper.Cos(rotationZ);
        var s1 = MathHelper.Sin(rotationX);
        var s2 = MathHelper.Sin(rotationY);
        var s3 = MathHelper.Sin(rotationZ);

        _xyz = new Vector3
        (
            s1 * c2 * c3 + c1 * s2 * s3,
            c1 * s2 * c3 - s1 * c2 * s3,
            c1 * c2 * s3 + s1 * s2 * c3
        );
        _w = c1 * c2 * c3 - s1 * s2 * s3;
    }

    public Quaternion(Vector3 eulerAngles) : this(eulerAngles.X, eulerAngles.Y, eulerAngles.Z)
    {
    }

    public float X
    {
        get => _xyz.X;
        set => _xyz.X = value;
    }

    public float Y
    {
        get => _xyz.Y;
        set => _xyz.Y = value;
    }

    public float Z
    {
        get => _xyz.Z;
        set => _xyz.Z = value;
    }

    public Vector4 ToAxisAngle()
    {
        var quaternion = this;
        if (MathHelper.Abs(quaternion.W) > 1) quaternion.Normalize();

        var result = new Vector4
        {
            W = 2 * MathHelper.Acos(quaternion.W)
        };
        var den = MathHelper.Sqrt(1 - quaternion.W * quaternion.W);

        if (den > 0.0001f) result.Xyz = quaternion.Xyz / den;
        else result.Xyz = Vector3.UnitX;

        return result;
    }

    public void ToAxisAngle(out Vector3 axis, out float angle)
    {
        var result = ToAxisAngle();
        axis = result.Xyz;
        angle = result.W;
    }

    public float Length
        => MathHelper.Sqrt(W * W + Xyz.LengthSquared);

    public float LengthSquared
        => W * W + Xyz.LengthSquared;

    public Quaternion Normalized()
    {
        var q = this;
        q.Normalize();
        return q;
    }

    public void Normalize()
    {
        var scale = 1f / Length;
        Xyz *= scale;
        W *= scale;
    }

    public void Invert()
    {
        W = -W;
    }

    public void Conjugate()
    {
        Xyz = -Xyz;
    }

    public Quaternion Inverted()
    {
        var quaternion = this;
        quaternion.Invert();
        return quaternion;
    }

    public static Quaternion Identity => new(0, 0, 0, 1);

    public static Quaternion Add(Quaternion left, Quaternion right)
        => new(left.Xyz + right.Xyz, left.W + right.W);

    public static Quaternion Subtract(Quaternion left, Quaternion right)
        => new(left.Xyz - right.Xyz, left.W - right.W);

    public static Quaternion Multiply(Quaternion quaternion, float scale)
        => new Quaternion(quaternion.Xyz * scale, quaternion.W * scale);

    public static Quaternion Multiply(Quaternion left, Quaternion right)
    {
        return new Quaternion(
            right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
            left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz)
        );
    }

    public static Quaternion Negate(Quaternion quaternion)
        => new(-quaternion.Xyz, -quaternion.W);

    public static Quaternion Conjugate(Quaternion quaternion)
        => new(-quaternion.Xyz, quaternion.W);

    public static Quaternion Invert(Quaternion q)
    {
        var lengthSquared = q.LengthSquared;
        if (lengthSquared == 0) return q;
        var i = 1 / lengthSquared;
        return new Quaternion(q.Xyz * -i, q.W * i);
    }

    public static Quaternion Normalize(Quaternion quaternion)
    {
        var scale = 1 / quaternion.Length;
        return new Quaternion(quaternion.Xyz * scale, quaternion.W * scale);
    }

    public static Quaternion FromAxisAngle(Vector3 axis, float angle)
    {
        if (axis.LengthSquared == 0) return Identity;
        Quaternion result;
        angle *= 0.5f;
        axis.Normalize();
        result = new Quaternion(
            axis * MathHelper.Sin(angle),
            MathHelper.Cos(angle)
        );

        return Normalize(result);
    }

    public static Quaternion FromEulerAngles(float pitch, float yaw, float roll)
        => new(pitch, yaw, roll);

    public static Quaternion FromEulerAngles(Vector3 eulerAngles)
        => new(eulerAngles);

    public static Quaternion FromMatrix(Matrix3 matrix)
    {
        var trace = matrix.Trace;

        if (trace > 0)
        {
            var s = MathHelper.Sqrt(trace + 1) * 2;
            var invS = 1 / s;

            return new Quaternion
            (
                (matrix.M32 - matrix.M23) * invS,
                (matrix.M13 - matrix.M31) * invS,
                (matrix.M21 + matrix.M12) * invS,
                s * 0.25f
            );
        }

        var m00 = matrix.M11;
        var m11 = matrix.M22;
        var m22 = matrix.M33;

        if (m00 > m11 && m00 > m22)
        {
            var s = MathHelper.Sqrt(1 + m00 - m11 - m22) * 2;
            var invS = 1 / s;

            return new Quaternion
            (
                s * 0.25f,
                (matrix.M12 + matrix.M21) * invS,
                (matrix.M13 + matrix.M31) * invS,
                (matrix.M32 - matrix.M23) * invS
            );
        }

        if (m11 > m22)
        {
            var s = MathHelper.Sqrt(1 + m11 - m00 - m22) * 2;
            var invS = 1 / s;

            return new Quaternion
            (
                (matrix.M12 + matrix.M21) * invS,
                s * 0.25f,
                (matrix.M23 + matrix.M32) * invS,
                (matrix.M13 + matrix.M31) * invS
            );
        }
        else
        {
            var s = MathHelper.Sqrt(1 + m22 - m00 - m11) * 2;
            var invS = 1 / s;

            return new Quaternion
            (
                (matrix.M13 + matrix.M31) * invS,
                (matrix.M23 + matrix.M32) * invS,
                s * 0.25f,
                (matrix.M21 - matrix.M12) * invS
            );
        }
    }

    public static Quaternion Slerp(Quaternion quaternionA, Quaternion quaternionB, float blend)
    {
        if (quaternionA.LengthSquared == 0)
            return quaternionB.LengthSquared == 0 ? Identity : quaternionB;

        if (quaternionB.LengthSquared == 0)
            return quaternionA;

        var cosHalfAngle
            = quaternionA.W * quaternionB.W + Vector3.Dot(quaternionA.Xyz, quaternionB.Xyz);

        switch (cosHalfAngle)
        {
            case >= 1.0f:
            case <= -1.0f:
                return quaternionA;
            case < 0:
                quaternionB = Negate(quaternionB);
                cosHalfAngle = -cosHalfAngle;
                break;
        }

        float blendA, blendB;
        if (cosHalfAngle < 0.99f)
        {
            var halfAngle = MathHelper.Acos(cosHalfAngle);
            var sinHalfAngle = MathHelper.Sin(halfAngle);
            var oneOverSinHalfAngle = 1.0f / sinHalfAngle;

            blendA = MathHelper.Sin((1.0f - blend) * halfAngle) * oneOverSinHalfAngle;
            blendB = MathHelper.Sin(blend * halfAngle) * oneOverSinHalfAngle;
        }
        else
        {
            blendA = 1.0f - blend;
            blendB = blend;
        }

        var result = new Quaternion
        (
            blendA * quaternionA.Xyz + blendB * quaternionB.Xyz,
            blendA * quaternionA.W + blendB * quaternionB.W
        );

        return result.LengthSquared > 0 ? Normalize(result) : Identity;
    }

    public static Quaternion operator +(Quaternion left, Quaternion right) => Add(left, right);
    public static Quaternion operator -(Quaternion left, Quaternion right) => Subtract(left, right);
    public static Quaternion operator -(Quaternion left) => Negate(left);
    public static Quaternion operator *(Quaternion left, Quaternion right) => Multiply(left, right);
    public static Quaternion operator *(Quaternion left, float right) => Multiply(left, right);
    public static Quaternion operator *(float left, Quaternion right) => Multiply(right, left);

    public static bool operator ==(Quaternion left, Quaternion right) => left.Equals(right);
    public static bool operator !=(Quaternion left, Quaternion right) => !left.Equals(right);

    public override string ToString()
        => $"V: {_xyz},  W: {W}";

    public override int GetHashCode()
        => HashCode.Combine(_xyz, W);

    public bool Equals(Quaternion other)
        => Xyz.Equals(other.Xyz) && W.Equals(other.W);

    public override bool Equals(object? obj)
        => obj is Quaternion other && Equals(other);
}